/*
MPU 9150/9250 Sample library v0.1

todo:
分割读取代码
分析磁力传感器


*/
#include "es_iic.h"

#define masterAddress 0xa
#define slaverAddress 0xb

#define mpu9150_addr 0xD0
#define FIFO_reg 116

#define MOTION_THREDHOLD 0.001
struct packet
{
	u16 accX;
	u16 accY;
	u16 accZ;

	u16 gyroX;
	u16 gyroY;
	u16 gyroZ;

	u16 magX;
	u16 magY;
	u16 magZ;
};

class mpu9150
{
public:
	u8 gyroXH;
	u8 gyroXL;
	u16 gyroX;
	u8 gyroYH;
	u8 gyroYL;
	u16 gyroY;
	u8 gyroZH;
	u8 gyroZL;
	u16 gyroZ;


	u8 magXH;
	u8 magXL;
	u16 magX;
	u8 magYH;
	u8 magYL;
	u16 magY;
	u8 magZH;
	u8 magZL;
	u16 magZ;


	u8 temphigh;
	u8 templow;
	u16 temp;

	u8 accXhigh;
	u8 accXlow;
	u8 accYhigh;
	u8 accYlow;
	u8 accZhigh;
	u8 accZlow;
	u16 accX;
	u16 accY;
	u16 accZ;

	volatile double aX;
	volatile double aY;
	volatile double aZ;

	volatile double gX;
	volatile double gY;
	volatile double gZ;

	double gOffsetX;
	double gOffsetY;
	double gOffsetZ;

	//用于软动态检测储存历史数据
	double avgX, avgY, avgZ;

	u8 data;

	u8 fucker1;
	u8 fucker2;

	void init(CI2C* i2c)
	{
		//init I2c bus
		master = i2c;
		//master->Init(i2c1, masterAddress);
		master->waitBus();

		//read device ID(who Am I)	0x75
		master->master_read_reg(mpu9150_addr, 117, data);

		//power setting	 0x6b
		master->master_write_reg(mpu9150_addr, 107, 0);

		// 0x6c
		master->master_write_reg(mpu9150_addr, 108, 0x07);

		//accel_config	0x1c
		master->master_write_reg(mpu9150_addr, 28, 0x00);

		//FIFO_enable
		//master->master_write_reg(mpu9150_addr, 35, 0x00);

		//sample rate divider	0x19
		master->master_write_reg(mpu9150_addr, 25, 0x00);
	
		//0x37
		master->master_write_reg(mpu9150_addr, 55, 0x12);

		//enable FIFO globally 	0x6a
		master->master_write_reg(mpu9150_addr, 106, 0x00);


		// 			for(int i = 0; i<100000;i++);
		//u8 fucker = 0x00;
		//master->master_read_reg(0x0C, 0, fucker1);
		//  			

		//  			master->master_write_reg(0x0C, 0x0A,  0x01);		
		// 			master->master_write_reg(mpu9150_addr, 55, 0x00);
		// 			for(int i = 0; i<10000; i++);

					//i2c_slv0 config for mag sensor
		// 			master->master_write_reg(mpu9150_addr, 37, 0x86);
		// 			master->master_write_reg(mpu9150_addr, 38, 0x03);
		// 			master->master_write_reg(mpu9150_addr, 39, 0x86);

	}

	//硬运动检测 配置
	void motionDetectionInit()
	{
		master->master_write_reg(mpu9150_addr, 55, 0x12);

		master->master_write_reg(mpu9150_addr, 29, 0x09);
		//enable int
		master->master_write_reg(mpu9150_addr, 56, 0x40);

		//enable motion detection
		master->master_write_reg(mpu9150_addr, 105, 0xC0);

		//set threhold
		master->master_write_reg(mpu9150_addr, 0x1F, 0x08);

		master->master_write_reg(mpu9150_addr, 0x1e, 0x11);

		master->master_write_reg(mpu9150_addr, 107, 0x20);
	}

	void read()
	{
		// 		u8 fifo_count;
		// 		master->master_read_reg(mpu9150_addr, 115, fifo_count);
		// 		if(fifo_count < 0x50)
		// 		master->master_read_reg(mpu9150_addr, 59, fucker1);
		// 		master->master_read_reg(mpu9150_addr, 60, fucker2);
					//read accerlation 加速度传感
		master->master_read_reg(mpu9150_addr, 59, accXhigh);
		master->master_read_reg(mpu9150_addr, 60, accXlow);
		accX = 0;
		accX = accXlow;
		accX |= accXhigh << 8;
		aX = (double)(short)accX / 16384;

		master->master_read_reg(mpu9150_addr, 61, accYhigh);
		master->master_read_reg(mpu9150_addr, 62, accYlow);
		accY = 0;
		accY = accYlow;
		accY |= accYhigh << 8;
		aY = (double)(short)accY / 16384;

		master->master_read_reg(mpu9150_addr, 63, accZhigh);
		master->master_read_reg(mpu9150_addr, 64, accZlow);
		accZ = 0;
		accZ = accZlow;
		accZ |= accZhigh << 8;
		aZ = (double)(short)accZ / 16384;

		//温度
		master->master_read_reg(mpu9150_addr, 65, temphigh);
		master->master_read_reg(mpu9150_addr, 66, templow);
		temp = (temphigh << 8) | templow;
		temp = 35 + ((double)(temp + 13200)) / 280;


		//gyro scope 陀螺仪
		master->master_read_reg(mpu9150_addr, 67, gyroXH);
		master->master_read_reg(mpu9150_addr, 68, gyroXL);
		gyroX = (gyroXH << 8) | gyroXL;
		gX = (double)(short)gyroX / 131.072;

		master->master_read_reg(mpu9150_addr, 69, gyroYH);
		master->master_read_reg(mpu9150_addr, 70, gyroYL);
		gyroY = (gyroYH << 8) | gyroYL;
		gY = (double)(short)gyroY / 131.072;

		master->master_read_reg(mpu9150_addr, 71, gyroZH);
		master->master_read_reg(mpu9150_addr, 72, gyroZL);
		gyroZ = (gyroZH << 8) | gyroZL;
		gZ = (double)(short)gyroZ / 131.072;

		//magneto 磁力传感
		master->master_read_reg(mpu9150_addr, 73, magXH);
		master->master_read_reg(mpu9150_addr, 74, magXL);
		magX = (magXH << 8) | magXL;

		master->master_read_reg(mpu9150_addr, 75, magYH);
		master->master_read_reg(mpu9150_addr, 76, magYL);
		magY = (magYH << 8) | magYL;

		master->master_read_reg(mpu9150_addr, 77, magZH);
		master->master_read_reg(mpu9150_addr, 78, magZL);
		magZ = (magZH << 8) | magZL;


		//calibrating
		gX -= gOffsetX;
		gY -= gOffsetY;
		gZ -= gOffsetZ;

	}

	void SoftwreMotionInit() 
	{
		avgX = aX;
		avgY = aY;
		avgZ = aZ;
	}

	//软运动检测更新， 应随read之后调用
	u8 softwareMotionUpdate() 
	{

		//计算差值
		double msd = (avgX - aX)*(avgX - aX) +
			(avgY - aY)*(avgY - aY) +
			(avgZ - aZ)*(avgZ - aZ);


		//差值超过0.001， 报告震动
		if (msd > MOTION_THREDHOLD)
		{

			avgX = aX;
			avgY = aY;
			avgZ = aZ;

			return true;
		}
		else
		{
			//更新历史, 学习速度是新数据占40%权重
			avgX = avgX*0.6 + aX*0.4;
			avgY = avgY*0.6 + aY*0.4;
			avgZ = avgZ*0.6 + aZ*0.4;

		}

		return false;
	}

	//read data for calibrating the gyroscope via subtracting the offset
	void gyroCalibration()
	{
		gOffsetX = gX;
		gOffsetY = gY;
		gOffsetZ = gZ;
	}

private:
	CI2C* master;
};
